% Florian Meyer, 22/04/15.

function [ normalizedGradient ] = calculateGradient(anchorPos, agentPosterior, observationValuesAnchors, observationValuesAgents, parameter)

%initialize parameters
[particleNum, ~, agentNum] = size(agentPosterior);
[anchorNum, ~] = size(anchorPos);
[~, observNum, ~] = size(observationValuesAnchors);
normalizedGradient = zeros(agentNum,2);
measurementVariance = parameter.measurementVariance;
measurementVariancePartner = measurementVariance/2;
maxVel = parameter.maxVel;
censoringVar = parameter.censoringVar;

%calculate the mutual information gradient for each mobile agent
for agent = 1:agentNum    
    numFactors = anchorNum;
    likelihoodMatrix = zeros(particleNum,observNum);
    likelihoodMatrix3D = zeros(particleNum,observNum,particleNum);
    normalizedGradientLoop = zeros(2,1);
    measurementVarianceAnchor = measurementVariance*ones(particleNum,observNum, anchorNum);
    measurementVarianceAgent = measurementVariancePartner*ones(particleNum,observNum, agentNum);
    
    %evaluate local likelihood function corresponding to each partner anchor at the particles
    for anchor = 1:anchorNum
        tmpDist = sqrt( (agentPosterior(:,1,agent)-anchorPos(anchor,1)).^2 + (agentPosterior(:,2,agent)-anchorPos(anchor,2)).^2 );
        if(parameter.pathloss)
            idx1 = tmpDist > parameter.distZero;
            measurementVarianceAnchor(idx1,:,anchor) = repmat(((tmpDist(idx1)/parameter.distZero - 1).^parameter.kappa + 1)*measurementVariance,1,observNum);
        end
        
        likelihoodMatrix = likelihoodMatrix - 1./(2*measurementVarianceAnchor(:,:,anchor)) .*  (observationValuesAnchors(:,:,anchor,agent) - repmat(tmpDist,1,observNum)).^2;
        
        state3Dpoints = repmat(permute(tmpDist,[3,2,1]),[particleNum observNum 1]);
        observations3Dpoints  = repmat(observationValuesAnchors(:,:,anchor,agent),[1,1,particleNum]);
        tmpV3D = repmat(permute(1./(2*measurementVarianceAnchor(:,1,anchor)),[3,2,1]),[particleNum observNum 1]);
        likelihoodMatrix3D = likelihoodMatrix3D - tmpV3D .* (observations3Dpoints - state3Dpoints).^2;
    end    
    
    agentUncertainty = mean(diag(cov(agentPosterior(:,:,agent))));    
    %perform censoring if agent is not properly localized; this means partner mobile agents are not considered
    if (agentUncertainty < censoringVar)
        numFactors = agentNum - 1 + anchorNum;
        
        %evaluate local likelihood function corresponding to each partner mobile agent at the particles
        for partnerAgent = 1:agentNum
            if(partnerAgent == agent)
                continue;
            end
            tmpDist = sqrt( (agentPosterior(:,1,agent)-agentPosterior(:,1,partnerAgent)).^2 + (agentPosterior(:,2,agent)-agentPosterior(:,2,partnerAgent)).^2 );
            if(parameter.pathloss)
                idx1 = tmpDist > parameter.distZero;
                measurementVarianceAgent(idx1,:,partnerAgent) = repmat(((tmpDist(idx1)/parameter.distZero - 1).^parameter.kappa + 1)*measurementVariancePartner,1,observNum);
            end
            likelihoodMatrix = likelihoodMatrix - 1/(2*measurementVariancePartner) *  (observationValuesAgents(:,:,partnerAgent,agent) - repmat(tmpDist,1,observNum)).^2;
            
            state3Dpoints = repmat(permute(tmpDist,[3,2,1]),[particleNum observNum 1]);
            observations3Dpoints  = repmat(observationValuesAgents(:,:,partnerAgent,agent),[1,1,particleNum]);
            tmpV3D = repmat(permute(1./(2*measurementVarianceAgent(:,1,partnerAgent)),[3,2,1]),[particleNum observNum 1]);
            likelihoodMatrix3D = likelihoodMatrix3D - tmpV3D .* (observations3Dpoints - state3Dpoints).^2;
        end
    end
    
    %evaluate global likelihood function f(y^+|x) at the particles
    tmpV = 1./sqrt(prod(measurementVarianceAnchor,3).*prod(measurementVarianceAgent,3));
    likelihoodMatrix = (1/(sqrt(pi * 2)))^numFactors.*tmpV.*exp(likelihoodMatrix);
    
    %evaluate the global distribution of the measurements f(y^+) at the particles
    tmpV3D = repmat(permute(tmpV(:,1),[3,2,1]),[particleNum observNum 1]);
    likelihoodMatrix3D = (1/(sqrt(pi * 2)))^numFactors.*tmpV3D.*exp(likelihoodMatrix3D);
    fy = 1/particleNum*sum(likelihoodMatrix3D,3);
    
    %calculate the entries of the gradient separately
    for gradientIndex = 1:2
        likelihoodMatrixDer = zeros(particleNum,observNum);
        
        %evaluate the gradient of local likelihood functions related to future measurements performed to other anchors
        for anchor = 1:anchorNum
            tmpT = sqrt( (agentPosterior(:,1,agent)-anchorPos(anchor,1)).^2 + (agentPosterior(:,2,agent)-anchorPos(anchor,2)).^2 );
            tmpD = (observationValuesAnchors(:,:,anchor,agent) - repmat(tmpT,1,observNum));
            likelihoodMatrixDer = likelihoodMatrixDer +  1./(measurementVarianceAnchor(:,:,anchor)) .* tmpD .* (repmat(agentPosterior(:,gradientIndex,agent),1,observNum)-anchorPos(anchor,gradientIndex))./repmat(tmpT,1,observNum);
            
            if(parameter.pathloss)
                tmpV = measurementVariance*parameter.kappa/parameter.distZero * (repmat(tmpT,1,observNum)./parameter.distZero - 1).^(parameter.kappa-1).*(repmat(agentPosterior(:,gradientIndex,agent),1,observNum)-anchorPos(anchor,gradientIndex))./repmat(tmpT,1,observNum);
                likelihoodMatrixDerTmp =  1./(measurementVarianceAnchor(:,:,anchor)) .* (-tmpV./2 + (1./(2*measurementVarianceAnchor(:,:,anchor)) .* tmpV .* tmpD.^2));
                idx2 = (tmpT <= parameter.distZero);
                likelihoodMatrixDerTmp(idx2,:) = 0;
                likelihoodMatrixDer = likelihoodMatrixDer + likelihoodMatrixDerTmp;
            end
        end
        
        %evaluate the gradient of local likelihood functions related to future measurements performed to other mobile agents
        if (agentUncertainty < censoringVar)
            for partnerAgent = 1:agentNum
                if(partnerAgent == agent)
                    continue;
                end
                tmpT = sqrt( (agentPosterior(:,1,agent)-agentPosterior(:,1,partnerAgent)).^2 + (agentPosterior(:,2,agent)-agentPosterior(:,2,partnerAgent)).^2 );
                tmpD = observationValuesAgents(:,:,partnerAgent,agent) - repmat(tmpT,1,observNum);
                likelihoodMatrixDer = likelihoodMatrixDer + 1./(measurementVarianceAgent(:,:,partnerAgent)) .* tmpD .*(repmat(agentPosterior(:,gradientIndex,agent)-agentPosterior(:,gradientIndex,partnerAgent),1,observNum))./repmat(tmpT,1,observNum);
                
                if(parameter.pathloss)
                    tmpV = measurementVariancePartner*parameter.kappa/parameter.distZero * (repmat(tmpT,1,observNum)./parameter.distZero - 1).^(parameter.kappa-1).*(repmat(agentPosterior(:,gradientIndex,agent) -agentPosterior(:,gradientIndex,partnerAgent),1,observNum))./repmat(tmpT,1,observNum);
                    likelihoodMatrixDerTmp =  1./(measurementVarianceAgent(:,:,partnerAgent)) .* (-tmpV./2 + (1./(2*measurementVarianceAgent(:,:,partnerAgent)) .* tmpV .* tmpD.^2));
                    idx2 = (tmpT <= parameter.distZero);
                    likelihoodMatrixDerTmp(idx2,:) = 0;
                    likelihoodMatrixDer = likelihoodMatrixDer + likelihoodMatrixDerTmp;
                end
            end
        end
        
        %evaluate derivation of the mutual information
        mutualInfoMatrix = likelihoodMatrixDer.*log(likelihoodMatrix./fy);
        
        %perform Monte Carlo integration
        normalizedGradientLoop(gradientIndex) = sum(sum(mutualInfoMatrix,2));
    end
    
    %normalize gradient
    if(norm(normalizedGradientLoop) ~=0)
        normalizedGradientLoop = maxVel(agent)/norm(normalizedGradientLoop) * normalizedGradientLoop;
    end
    normalizedGradient(agent,:) = normalizedGradientLoop;
end
end